/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "voslam_util.h"

/* gcc -O2 -Wall -g -c -o voslam_util.o voslam_util.c */

void hpsrt(double *ra,int * indx, int n)
{
    unsigned long i,ir,j,k,ira;
    double rra;
    if (n < 2) return;
    k=(n >> 1);
    ir=n-1;
    for (;;) 
    {
        if(k > 0)
        { 
            rra=ra[--k];
            ira=indx[k];
        }
        else 
        {
            rra=ra[ir];
            ira=indx[ir];
            ra[ir]=ra[0];
            indx[ir]=indx[0];
            if (--ir == 0) 
            {
                ra[0]=rra; 
                indx[0]=ira; 
                break;
            }
        }
        i=k;
        j=k+1;
        while (j <= ir)
        {
            if (j < ir && ra[j] < ra[j+1]) j++;
            if (rra < ra[j])
            {
                ra[i]=ra[j];
                indx[i]=indx[j];  
                i=j;
                j <<= 1;
            } else break;
        }
        ra[i]=rra;
        indx[i]=ira;
    }
}
void jsort(double *ra, int n)
{
    unsigned long i,ir,j,k;
    double rra;
    if (n < 2) return;
    k=(n >> 1);
    ir=n-1;
    for (;;) 
    {
        if(k > 0)
        { 
            rra=ra[--k];
        }
        else 
        {
            rra=ra[ir];
            ra[ir]=ra[0];
            if (--ir == 0) 
            {
                ra[0]=rra; 
                break;
            }
        }
        i=k;
        j=k+1;
        while (j <= ir)
        {
            if (j < ir && ra[j] < ra[j+1]) j++;
            if (rra < ra[j])
            {
                ra[i]=ra[j]; 
                i=j;
                j <<= 1;
            } else break;
        }
        ra[i]=rra;
    }
}
/*min-heap remove and return minimum value node */
void up_heap(double *heap,int *indi, double val, int ind, int *N)
{
    unsigned int parent,child,tmpi;
    double tmp;

    heap[*N]=val;
    indi[*N]=ind;

    child=*N;
    (*N)++;
    if(*N==1)
        return;
    for(;;)
    {
        parent=(child-1)>>1;
        if(heap[parent]>heap[child])
        {
            tmp=heap[parent];
            heap[parent]=heap[child];
            heap[child]=tmp;
            tmpi=indi[parent];
            indi[parent]=indi[child];
            indi[child]=tmpi;

            if(parent==0)
                break;
        }
        else
            break;
        child=parent;
    }
}
void down_heap(double *heap,int *indi, double *val, int *ind, int *N)
{
    unsigned int parent,child1,child2,tmpi;
    double tmp;

    (*N)--;
    *val=heap[0];
    *ind=indi[0];
    heap[0]=heap[*N];
    indi[0]=indi[*N];

    parent=0;
    child1=(parent<<1)+1;
    child2=(parent<<1)+2;
    for(;;)
    {
        if(child1>(*N))
            break;
        if(child2>(*N))
            child2=child1;

        if(heap[child1]<heap[child2])
        {
            if(heap[child1]<heap[parent])
            {
                tmp=heap[parent];
                heap[parent]=heap[child1];
                heap[child1]=tmp;
                tmpi=indi[parent];
                indi[parent]=indi[child1];
                indi[child1]=tmpi;

                parent=child1;
                child1=(parent<<1)+1;
                child2=(parent<<1)+2;
            }
            else
                break;
        }
        else
        { 
            if(heap[child2]<heap[parent])
            {
                tmp=heap[parent];
                heap[parent]=heap[child2];
                heap[child2]=tmp;
                tmpi=indi[parent];
                indi[parent]=indi[child2];
                indi[child2]=tmpi;
                parent=child2;
                child1=(parent<<1)+1;
                child2=(parent<<1)+2;
            }
            else
                break;
        }
    }
}

void Rt_2_t(double *vout,double *vin)
{
  double R[9];
  
  quater_2_rot(R, vin);
  vout[0]=vin[0];
  vout[1]=vin[1];
  vout[2]=vin[2];
  vout[3]=vin[3];
  vout[4]=R[0]*vin[4]+R[3]*vin[5]+R[6]*vin[6];
  vout[5]=R[1]*vin[4]+R[4]*vin[5]+R[7]*vin[6];
  vout[6]=R[2]*vin[4]+R[5]*vin[5]+R[8]*vin[6];
}
void t_2_Rt(double *vout,double *vin)
{
  double R[9];
  
  quater_2_rot(R, vin);
  vout[0]=vin[0];
  vout[1]=vin[1];
  vout[2]=vin[2];
  vout[3]=vin[3];
  vout[4]=R[0]*vin[4]+R[1]*vin[5]+R[2]*vin[6];
  vout[5]=R[3]*vin[4]+R[4]*vin[5]+R[5]*vin[6];
  vout[6]=R[6]*vin[4]+R[7]*vin[5]+R[8]*vin[6];
}
void quater_2_rot(double *R, double *qo)
{
    double q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,q[4],norm;
    
    norm=sqrt(qo[0]*qo[0]+qo[1]*qo[1]+qo[2]*qo[2]+qo[3]*qo[3]);
    q[0]=qo[0]/norm; 
    q[1]=qo[1]/norm;
    q[2]=qo[2]/norm;
    q[3]=qo[3]/norm;
   
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    //building rotation matrix

    R[0]=(q1t2+q2t2-q3t2-q4t2);
    R[1]=2*(q2q3+q1q4);
    R[2]=2*(q2q4-q1q3);
    R[3]=2*(q2q3-q1q4);
    R[4]=(q1t2-q2t2+q3t2-q4t2);
    R[5]=2*(q3q4+q1q2);
    R[6]=2*(q2q4+q1q3);
    R[7]=2*(q3q4-q1q2);
    R[8]=(q1t2-q2t2-q3t2+q4t2);
}


void rot_2_quater(double *q,double *R)
{
    double theta,om[3],norm,trac;
    
    trac=(R[0]+R[4]+R[8]-1)/2.;
    theta=0;
    if(fabs(trac)<=1.)
    {
        theta=acos(trac);
   
        om[0]=R[5]-R[7];
        om[1]=R[6]-R[2];
        om[2]=R[1]-R[3];
        q[0]=cos(theta/2);
        norm=sqrt(om[0]*om[0]+om[1]*om[1]+om[2]*om[2]);
        if(norm>0.00000001)
        {
            q[1]=om[0]/norm*sin(theta/2.);    
            q[2]=om[1]/norm*sin(theta/2.);
            q[3]=om[2]/norm*sin(theta/2.);
        }
        else
        {
            q[1]=0.;
            q[2]=0.;
            q[3]=0.;
        }
    }
    else
    {
        q[0]=1.;
        q[1]=0.;
        q[2]=0.;
        q[3]=0.;
    }
}
void skew_2_quater(double *q,double theta, double *om)
{
    double norm;
    

    if(fabs(theta)>0.0000000001)
    {

        q[0]=cos(theta/2);
        norm=sqrt(om[0]*om[0]+om[1]*om[1]+om[2]*om[2]);
        if(norm>0.00000001)
        {
            q[1]=om[0]/norm*sin(theta/2.);    
            q[2]=om[1]/norm*sin(theta/2.);
            q[3]=om[2]/norm*sin(theta/2.);
        }
        else
        {
            q[1]=0.;
            q[2]=0.;
            q[3]=0.;
        }
    }
    else
    {
        q[0]=1.;
        q[1]=0.;
        q[2]=0.;
        q[3]=0.;
    }
}
void quater_2_skew(double *theta, double *om,double *q)
{
    double norm;
    
    theta[0]=acos(q[0])*2;
    norm=sqrt(q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    
    om[0]=q[1]/norm;    
    om[1]=q[2]/norm;
    om[2]=q[3]/norm;
    
}
void quaterrot(double *q3,double *q1,double *q2)
{
    q3[0]=q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
    q3[1]=q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
    q3[2]=q1[0]*q2[2]-q1[1]*q2[3]+q1[2]*q2[0]+q1[3]*q2[1];
    q3[3]=q1[0]*q2[3]+q1[1]*q2[2]-q1[2]*q2[1]+q1[3]*q2[0];
}
